#include "NavigationEdgeList.h"
#include "NavigationEdge.h"

NavigationEdgeList::NavigationEdgeList()
{
	head = NULL;
	length = 0;
}
NavigationEdgeList::NavigationEdgeList(NavigationEdge * headNavigationEdge)
{
	head = new List<NavigationEdge>(headNavigationEdge, NULL);
	head->next = NULL;
	head->prev = NULL;
	length = 1;
}
NavigationEdgeList::~NavigationEdgeList()
{
	//printf("deleting navigation edge list\n");
	List<NavigationEdge> * search = head;
	while(search)
	{
		search = search->Remove();
	}
	head = NULL;
	//printf("done deleting navigation edge list\n");
}
List<NavigationEdge> * NavigationEdgeList::Add(NavigationEdge * payload)
{
	//printf("pre head = %p\n", head);
	length++;
	head = new List<NavigationEdge>(payload, head);
	head->prev = NULL;
	//printf("post head = %p\n", head);
	return head;
}
NavigationEdge * NavigationEdgeList::Pop()
{
	if(head)
	{
		length--;
		List<NavigationEdge> * result = head;
		head = head->next;
		if(head)
			head->prev = NULL;
		NavigationEdge * edge = result->payload;
		result->payload = NULL;
		delete(result);
		result = NULL;
		return edge;
	}
	return NULL;
}
void NavigationEdgeList::Remove(NavigationEdge * point)
{
	List<NavigationEdge> * search = head;
	while(search)
	{
		if(search->payload && search->payload ==  point)
		{
			length--;
			search = search->Remove();
			return;
		}
		else
			search = search->next;
	}	
}
int NavigationEdgeList::GetLength()
{
	return length;
}
NavigationEdge * NavigationEdgeList::GetCurrentEdge(Vector2D position, float radius, NavigationEdge * desired)
{
	List<NavigationEdge> * search = head;
	float min = 9999;
	NavigationEdge * minEdge = NULL;
	while(search)
	{
		if(search->payload)
		{
			//does the circle overlap the edge?
			Vector2D rightEndPoint = search->payload->GetRightEndPoint();
			Vector2D leftEndPoint = search->payload->GetLeftEndPoint();
			float distance = Vector2D::PointToLineDistance(position, rightEndPoint, leftEndPoint);
			if((distance <= radius && distance < min))
			{
				min = distance;
				minEdge = search->payload;
			}
			if((distance <= radius && (search->payload == desired)))
			{
				return search->payload;
			}
		}
		search = search->next;
	}	
	return minEdge;
}

bool NavigationEdgeList::IsEmpty()
{
	return head==NULL;
}
NavigationEdge * NavigationEdgeList::PriorityPop()
{
	//printf("NavigationEdgeList::PriorityPop %d\n", length);
	float minF = 9999;
	List<NavigationEdge> * min = NULL;
	List<NavigationEdge> * search = head;
	while(search)
	{
		
		if(search->payload && search->payload->f < minF)
		{
			//printf("searching %f\n",search->payload->f);
			minF = search->payload->f;
			min = search;
		}
		//else
			//printf("priority pop searching\n");
		search = search->next;
	}
	//printf("while done\n");
	if(min)
	{
		NavigationEdge * result = min->payload;
		
		if(min->prev)
			min->prev->next = min->next;
		if(min->next)
			min->next->prev = min->prev;
		min->payload = NULL;
		if(min == head)
			head = NULL;
		delete(min);
		min = NULL;
		length--;
		//printf("NavigationEdgeList::PriorityPop done\n");
		return result;
	}
	//printf("NavigationEdgeList::PriorityPop null\n");
	return NULL;
}
void NavigationEdgeList::ConnectEdge(NavigationEdge * edge)
{
	List<NavigationEdge> * search = head;
	while(search)
	{
		if(search->payload)
		{
			#define CLOSE_DISTANCE 0.01
			if((edge->GetRightEndPoint() | search->payload->GetRightEndPoint()) < CLOSE_DISTANCE)
			{
				edge->AddRightEdge(search->payload);
				search->payload->AddRightEdge(edge);
			}
			else if((edge->GetRightEndPoint() | search->payload->GetLeftEndPoint()) < CLOSE_DISTANCE)
			{
				edge->AddRightEdge(search->payload);
				search->payload->AddLeftEdge(edge);
			}
			else if((edge->GetLeftEndPoint() | search->payload->GetLeftEndPoint()) < CLOSE_DISTANCE)
			{
				edge->AddLeftEdge(search->payload);
				search->payload->AddLeftEdge(edge);
			}
			else if((edge->GetLeftEndPoint() | search->payload->GetRightEndPoint()) < CLOSE_DISTANCE)
			{
				edge->AddLeftEdge(search->payload);
				search->payload->AddRightEdge(edge);
			}
		}
		search = search->next;
	}	
	Add(edge);
}
/************************************************************************************************
 *	BuildGraph
 * 	Input: NavigationEdgeList * edges --> 	list must be "properly formed" i.e. a junction must
 * 											contain at least one rightEndPoint and one leftEndPoint
 * 	Output: NavigationEdge * -->	This is a pointer to the head of the edgeList which is now in a
 * 									graph with its endpoint lists connected
 * **********************************************************************************************/
NavigationEdgeList * NavigationEdgeList::BuildGraph(NavigationEdgeList * edges)
{
	//printf("NavigationEdgeList::BuildGraph\n");
	NavigationEdgeList * graph = NULL;
	if(edges)
	{
		//printf("start edges length = %d\n",(edges)->GetLength());
		NavigationEdge * edge = (edges)->Pop();
		graph = new NavigationEdgeList(edge);
		
		edge = (edges)->Pop();
		while(edge)
		{
			graph->ConnectEdge(edge);
			edge = (edges)->Pop();
		}
	}
		
	return graph;
}
bool NavigationEdgeList::Contains(NavigationEdge * edge)
{
	//printf("NavigationEdgeList::Contains %d\n", length);
	List<NavigationEdge> * search = head;
	while(search)
	{	
		//printf("here1\n");
		if(search->payload == edge)
		{
			//printf("Returning true\n");
			return true;
		}
		//printf("here2\n");
		search = search->next;
	}
	return false;
}
void NavigationEdgeList::Merge(NavigationEdgeList * list)
{
	//printf("NavigationEdgeList::Merge\n");
	if(list)
	{
		List<NavigationEdge> * search = list->head;
		while(search)
		{
			if(search->payload)
			{
				//printf("Merging\n");
				Add(search->payload);
			}
			search = search->next;
		}
		//printf("here\n");
	}
	//printf("Done NavigationEdgeList::Merge\n");
}
void NavigationEdgeList::Clear()
{
	length = 0;
	List<NavigationEdge> * search = head;
	while(search)
	{
		search = search->RemoveList();
	}
	head = NULL;
}
void NavigationEdgeList::RunFunction(void (NavigationEdge::*func)())
{
	List<NavigationEdge> * search = head;
	while(search)
	{
		if(search->payload)
			(search->payload->*func)();
		search = search->next;
	}
}
